#ifndef __LIO_H__
#define __LIO_H__

#define LIO_DEBUG
#include <time.h>
#include <iostream>
#include <queue>
#include <mutex>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "so3_math.h"
#include "use_ikfom.hpp"

#include "ivox3d/ivox3d.h"

#include "ouster_point_type.h"

namespace slam
{

    using PointType = PointOuster;
    using PointCloudType = pcl::PointCloud<PointType>;
    using IVoxType = IVox<3, IVoxNodeType::DEFAULT, PointType>;

    class Lio
    {
    private:
        // lio state, before recieved enough IMU data, the lio is INITIALIZING
        typedef enum
        {
            INITIALIZING = 0,
            RUNNING = 1
        } lio_state_t;

        lio_state_t lio_state_;

        // error-state extended kalman filter, for maintaining lio state
        esekfom::esekf<state_ikfom, 12, input_ikfom> kf_;
        state_ikfom state_;

        // the current lio state corresponding time
        double state_time_;

        input_ikfom state_input_;

        // imu data type
        typedef struct
        {
            double time;
            Eigen::Vector3d angular_velocity;
            Eigen::Vector3d acceleration;
        } imu_data_t;

        // lidar data type
        typedef struct
        {
            double time;
            PointCloudType::Ptr pointcloud;
        } lidar_data_t;

        // data buffers for storing incoming data
        std::queue<imu_data_t> imu_data_buffer_;
        std::queue<lidar_data_t> lidar_data_buffer_;

        uint32_t minimum_initial_imu_data_;
        uint32_t ikfom_max_iterations_;

        Eigen::Matrix<slam::process_noise_ikfom::scalar, 12, 12> noise_covariance;
        Eigen::Vector3d initial_lidar_translation_wrt_imu_;
        Eigen::Matrix3d initial_lidar_rotation_wrt_imu_;

        std::function<void(state_ikfom &, esekfom::dyn_share_datastruct<double> &)> observation_function_;

        const double GRAVITY = 9.81;
        double acceleration_normalizer_;

        std::mutex mutex_;

        double range_lower_limit_;
        double range_upper_limit_;

        std::shared_ptr<IVoxType> ivox_;

        double downsampling_size_;

        lidar_data_t current_lidar_data_;
        std::vector<std::tuple<Eigen::Vector3f, Eigen::Vector4f, float>> nearest_point_norm_residual_;
        std::vector<int> points_add_to_map_;

        double lidar_covariance_;
        int nearest_points_number_;
        int minimum_nearest_points_;
        double max_nearest_points_search_range_;

        Eigen::Matrix4d initial_transform;
        double valid_rate;

#ifdef LIO_DEBUG
        int frames_;
        int avg_time_;
        int max_time_;
#endif // LIO_DEBUG

        // ----raistlin add
        esekfom::esekf<state_ikfom, 12, input_ikfom> kf2_;         // used in InitializeKF_(), Run(), GenerateResults()
        std::list<imu_data_t> imu_data_buffer2_;                   // updated in PushImuData(), used in GenerateResults()
        size_t imu_data_i2_;                                       // used in GenerateResults()
        std::list<imu_data_t>::const_iterator imu_data_iter2_;     // used in GenerateResults()
        std::list<imu_data_t>::const_iterator imu_data_iter2_pre_; // used in GenerateResults()
        double lidar_time_;                                        // updated in Run()
        double imu_time_;                                          // updated in Run() & GenerateResults()
        int lidar_updated_;                                        // updated in Run(), used in GenerateResults()
        // ----raistlin add

    private:
        void ObservationModel_(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data);

        void RemoveOutlierPoints_(const PointCloudType::Ptr &pointcloud);

        void InitializeKF_();

        bool EstimatePlane_(Eigen::Matrix<float, 4, 1> &pca_result, const IVoxType::PointVector &point, const float &threshold);

        void PropagateForward_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list, double last_point_time);

        void UndistortPointCloud_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list, lidar_data_t &pointcloud);

        void Mapping();

        // ----raistlin add
        bool CheckValidity_(state_ikfom &s);
        // ----raistlin add
        //--------DJCadd
        bool CheckIMU_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list);
        //--------DJCadd
    public:
        Lio(std::shared_ptr<IVoxType> &ivox);
        Lio(std::shared_ptr<IVoxType> &ivox, const Eigen::Matrix4d &initial_transform);
        ~Lio();

        // imu input
        void PushImuData(double t, Eigen::Vector3d angular_velocity, Eigen::Vector3d acceleration);

        // pointcloud input
        void PushPointcloud(double t, const PointCloudType::Ptr pointcloud);

        // state predict
        int Run(PointCloudType::Ptr &pointcloud, Eigen::Vector3d &translation, Eigen::Quaterniond &rotation, Eigen::Vector3d &velocity);
        
        void millisleep(unsigned int milliseconds);

        // ----raistlin add
        bool GenerateResults(double &d_time, Eigen::Vector3d &v_pos, Eigen::Quaterniond &q_rot, Eigen::Vector3d &v_vel);
        void CheckBuffers(size_t &lidar_size, size_t &imu_size, size_t &imu2_size, size_t &imu2_i);
        void CheckBuffers(size_t &lidar_size, size_t &imu_size, size_t &imu2_size, size_t &imu2_i, double &lidar_time, double &imu_time, double &imu2_time);
        // ----raistlin add
    };

} // namespace slam

#endif // __LIO_H__
